#include "stdafx.h"


#include <assert.h>

#include "ImageUtils.h"

//#include "MathUtils.h"

#include "Debug.h"



//#include "ConfigManager.h"


CvScalar markerColors(int i)
{
	if (i==0) return cvScalar(255, 255, 192);
	if (i==1) return cvScalar(255, 192, 255);
	if (i==2) return cvScalar(192, 255, 255);

	if (i==3) return cvScalar(255, 192, 192);
	if (i==4) return cvScalar(192, 255, 192);
	if (i==5) return cvScalar(192, 192, 255);

	if (i==6) return cvScalar(255, 255, 127);
	if (i==7) return cvScalar(255, 127, 255);
	if (i==8) return cvScalar(127, 255, 255);

	if (i==9) return cvScalar(255, 127, 127);
	if (i==10) return cvScalar(127, 255, 127);
	if (i==11) return cvScalar(127, 127, 255);

	if (i==12) return cvScalar(255, 255,  64);
	if (i==13) return cvScalar(255,  64, 255);
	if (i==14) return cvScalar( 64, 255, 255);

	if (i==15) return cvScalar(255,  64,  64);
	if (i==16) return cvScalar( 64, 255,  64);
	if (i==17) return cvScalar( 64,  64, 255);

	if (i==18) return cvScalar(255, 255,   0);
	if (i==19) return cvScalar(255,   0, 255);
	if (i==20) return cvScalar(  0, 255, 255);

	if (i==21) return cvScalar(255,   0,   0);
	if (i==22) return cvScalar(  0, 255,   0);
	if (i==23) return cvScalar(  0,   0, 255);

	if (i==24) return cvScalar(192, 192, 127);
	if (i==25) return cvScalar(192, 127, 192);
	if (i==26) return cvScalar(127, 192, 192);

	if (i==27) return cvScalar(192, 127, 127);
	if (i==28) return cvScalar(127, 192, 127);
	if (i==29) return cvScalar(127, 127, 192);

	if (i==30) return cvScalar(192, 192,  64);
	if (i==31) return cvScalar(192,  64, 192);
	if (i==32) return cvScalar( 64, 192, 192);

	if (i==33) return cvScalar(192,  64,  64);
	if (i==34) return cvScalar( 64, 192,  64);
	if (i==35) return cvScalar( 64,  64, 192);

	if (i==36) return cvScalar(192, 192,   0);
	if (i==37) return cvScalar(192,   0, 192);
	if (i==38) return cvScalar(  0, 192, 192);

	if (i==39) return cvScalar(192,   0,   0);
	if (i==40) return cvScalar(  0, 192,   0);
	if (i==41) return cvScalar(  0,   0, 192);

	if (i==42) return cvScalar(127, 127,   0);
	if (i==43) return cvScalar(127,   0, 127);
	if (i==44) return cvScalar(  0, 127, 127);

	if (i==45) return cvScalar(127,   0,   0);
	if (i==46) return cvScalar(  0, 127,   0);
	if (i==47) return cvScalar(  0,   0, 127);

	if (i==48) return cvScalar(127, 127,  64);
	if (i==49) return cvScalar(127,  64, 127);
	if (i==50) return cvScalar( 64, 127, 127);

	if (i==51) return cvScalar(127,  64,  64);
	if (i==52) return cvScalar( 64, 127,  64);
	if (i==53) return cvScalar( 64,  64, 127);

	if (i==54) return cvScalar( 64,  64,   0);
	if (i==55) return cvScalar( 64,   0,  64);
	if (i==56) return cvScalar(  0,  64,  64);

	if (i==57) return cvScalar( 64,   0,   0);
	if (i==58) return cvScalar(  0,  64,   0);
	if (i==59) return cvScalar(  0,   0,  64);

	return cvScalar(  0,   0,  0);
}


bool ImageUtils::initialized = false;
/* This does not work with Visual Studio 2002
CvScalar ImageUtils::markerColors[MAX_MARKER_COLORS] = {

            cvScalar(255, 255, 192),
            cvScalar(255, 192, 255),
            cvScalar(192, 255, 255),

            cvScalar(255, 192, 192),
            cvScalar(192, 255, 192),
            cvScalar(192, 192, 255),

            cvScalar(255, 255, 127),
            cvScalar(255, 127, 255),
            cvScalar(127, 255, 255),

            cvScalar(255, 127, 127),
            cvScalar(127, 255, 127),
            cvScalar(127, 127, 255),

            cvScalar(255, 255,  64),
            cvScalar(255,  64, 255),
            cvScalar( 64, 255, 255),

            cvScalar(255,  64,  64),
            cvScalar( 64, 255,  64),
            cvScalar( 64,  64, 255),

            cvScalar(255, 255,   0),
            cvScalar(255,   0, 255),
            cvScalar(  0, 255, 255),

            cvScalar(255,   0,   0),
            cvScalar(  0, 255,   0),
            cvScalar(  0,   0, 255),


            cvScalar(192, 192, 127),
            cvScalar(192, 127, 192),
            cvScalar(127, 192, 192),

            cvScalar(192, 127, 127),
            cvScalar(127, 192, 127),
            cvScalar(127, 127, 192),

            cvScalar(192, 192,  64),
            cvScalar(192,  64, 192),
            cvScalar( 64, 192, 192),

            cvScalar(192,  64,  64),
            cvScalar( 64, 192,  64),
            cvScalar( 64,  64, 192),

            cvScalar(192, 192,   0),
            cvScalar(192,   0, 192),
            cvScalar(  0, 192, 192),

            cvScalar(192,   0,   0),
            cvScalar(  0, 192,   0),
            cvScalar(  0,   0, 192),


            cvScalar(127, 127,   0),
            cvScalar(127,   0, 127),
            cvScalar(  0, 127, 127),

            cvScalar(127,   0,   0),
            cvScalar(  0, 127,   0),
            cvScalar(  0,   0, 127),

            cvScalar(127, 127,  64),
            cvScalar(127,  64, 127),
            cvScalar( 64, 127, 127),

            cvScalar(127,  64,  64),
            cvScalar( 64, 127,  64),
            cvScalar( 64,  64, 127),


            cvScalar( 64,  64,   0),
            cvScalar( 64,   0,  64),
            cvScalar(  0,  64,  64),

            cvScalar( 64,   0,   0),
            cvScalar(  0,  64,   0),
            cvScalar(  0,   0,  64)
        };
*/


ImageUtils::ImageUtils() {}

ImageUtils::~ImageUtils() {}



float ImageUtils::maxi(float a, float b, float c) {
    if(a>=b && a>=c)
        return a;
    if(b>=a && b>=c)
        return b;
    if(c>=a && c>=b)
        return c;

    return a;
}



float ImageUtils::mini(float a, float b, float c) {
    if(a<=b && a<=c)
        return a;
    if(b<=a && b<=c)
        return b;
    if(c<=a && c<=b)
        return c;

    return a;
}


float ImageUtils::fitRange(float value, float low, float high){
	return mini(high, maxi(low, value));
}




///TODO genereller machen
void ImageUtils::copyRegion(IplImage* src, IplImage* dst, CvPoint start, CvPoint end) {


    CvSize imgSize;

    imgSize = cvSize(src->width, src->height); //TODO global machen!

    int x_count;
    int y_count;

    start.x = start.x >= imgSize.width  ? imgSize.width - 1  : start.x;
    end.x   = end.x   >= imgSize.width  ? imgSize.width - 1  : end.x;
    start.y = start.y >= imgSize.height ? imgSize.height - 1 : start.y;
    end.y   = end.y   >= imgSize.height ? imgSize.height - 1 : end.y;


    x_count = (end.x - start.x);
    y_count = (end.y - start.y);


    uchar* temp_ptr_x;
    uchar* temp_ptr_y;
    uchar* temp_ptr_x_input;
    uchar* temp_ptr_y_input;



    for (int y = 0; y <= y_count; y++) {

        temp_ptr_y       = ((uchar*)(dst->imageData + dst->widthStep * (start.y + y )));
        temp_ptr_y_input = ((uchar*)(src->imageData + src->widthStep * (start.y + y )));

        for (int x = 0; x <= x_count; x++) {

            temp_ptr_x       = &temp_ptr_y      [start.x * src->nChannels + x * src->nChannels];
            temp_ptr_x_input = &temp_ptr_y_input[start.x * src->nChannels + x * src->nChannels];

            temp_ptr_x[0] = temp_ptr_x_input[0];
            temp_ptr_x[1] = temp_ptr_x_input[1];
            temp_ptr_x[2] = temp_ptr_x_input[2];



        }
    }
}

bool ImageUtils::isSkinColor(Pixel pixel){
	return isSkinColor(pixel.getB(), pixel.getG(), pixel.getR());	
	
}


bool ImageUtils::isSkinColor(int b, int g, int r) {

	bool rgb;
	// bool hsv;   TODO Variables are not used  !!!!!!!!!
	// bool ycbcr; TODO Variables are not used  !!!!!!!!!
	
	rgb   = isSkinColorRGB(b,g,r);
	//hsv   = isSkinColorHSV(b,g,r);
	//ycbcr = isSkinColorYCbCr(b,g,r);

	//return ((rgb && hsv) || (hsv && ycbcr) || (rgb && ycbcr));

	return rgb;
	//return (rgb || hsv || ycbcr);
   
}

void ImageUtils::setPixelValue(IplImage* image, int x, int y, int channel, int value) {
    assert(channel >= 0);
    assert(channel < image->nChannels);
    assert(x >= 0);
    assert(y >= 0);
    assert(x < image->width);
    assert(y < image->height);
    (((uchar*)(image->imageData + image->widthStep*(y))))[x * image->nChannels + channel] = (uchar)value;
}



void ImageUtils::setPixelValue(IplImage* image, CvPoint point, int channel, int value) {

    setPixelValue(image, point.x, point.y, channel, value);
}



void ImageUtils::setPixelValue(IplImage* image, int x, int y, int num_channels, int* value, int order) {
    assert(num_channels <= 4);
    assert(num_channels != 2);
    assert(num_channels > 0);
    assert(y >= 0 && x >= 0);
    assert(y < image->height && x < image->width);

    switch(num_channels) {
    case 1:
        setPixelValue(image, x, y, 0, value[0]);
        break;

    case 3:
        switch(order) {
        case ORDER_BGR:
            for (int chan = 0; chan < num_channels; chan++) {
                setPixelValue(image, x, y, chan, value[chan]);
            }
            break;

        case ORDER_RGB:
            for (int chan = 0; chan < num_channels; chan++) {
                setPixelValue(image, x, y, 2 - chan, value[chan]);
            }
            break;
        }
        break;

    case 4:
        switch(order) {
        case ORDER_BGR:
            for (int chan = 0; chan < 3; chan++) {
                setPixelValue(image, x, y, chan, value[chan]);
            }
            setPixelValue(image, x, y, 3, value[3]);
            break;


        case ORDER_RGB:
            for (int chan = 0; chan < 3; chan++) {
                setPixelValue(image, x, y, 2 - chan, value[chan]);
            }
            setPixelValue(image, x, y, 3, value[3]);
            break;
        }
        break;
    }


}


void ImageUtils::setPixelValue(IplImage* image, CvPoint point, int num_channels, int* value, int order) {
    setPixelValue(image, point.x, point.y, num_channels, value, order);
}





void ImageUtils::setPixel(IplImage* image, Pixel pixel) {

    //setPixelValue(image, pixel.x, pixel.y, image->nChannels, pixel.getRGB(), ORDER_RGB);
    for (int i = 0; i < image->nChannels; ++i){
    	switch(i){
    		case 0:
    			setPixelValue(image, pixel.x, pixel.y, 0, pixel.b);
    			break;
    		case 1:
    			setPixelValue(image, pixel.x, pixel.y, 1, pixel.g);
    			break;
    		case 2:
    			setPixelValue(image, pixel.x, pixel.y, 2, pixel.r);
    			break;
    		case 3:
    			setPixelValue(image, pixel.x, pixel.y, 3, pixel.alpha);
    			break;	
    		
    	}	
    }
    
    
}



void ImageUtils::removeStrayPixels(IplImage *src, IplImage *dst, int step) {
    int x, y;
    int count = 0;
    uchar* pixel;


    for (y = 1; y < src->height - 1; y += 1) {
        for (x = 1; x < src->width - 1; x += 1) {
            count = 0;
            if (!(getPixelValue(src, x, y) == 0)) {

                if(getPixelValue(src, x - 1, y - 1) > 0)
                    count++;
                if(getPixelValue(src, x,     y - 1) > 0)
                    count++;
                if(getPixelValue(src, x + 1, y - 1) > 0)
                    count++;
                if(getPixelValue(src, x - 1, y    ) > 0)
                    count++;
                if(getPixelValue(src, x + 1, y    ) > 0)
                    count++;
                if(getPixelValue(src, x - 1, y + 1) > 0)
                    count++;
                if(getPixelValue(src, x,     y + 1) > 0)
                    count++;
                if(getPixelValue(src, x + 1, y + 1) > 0)
                    count++;


                if (count <= 3) {
                    pixel = (&((uchar*)(dst->imageData + dst->widthStep*(y /*+ y1*/)))[(x/* + x1*/) * 3]);
                    pixel[0] = 0;
                    pixel[1] = 0;
                    pixel[2] = 0;
                }
            }
        }
    }
}



int ImageUtils::getPixelValue(IplImage *image, int x, int y) {
    int sum = 0;
    for (int chan = 0; chan < image->nChannels; chan++) {
        sum += getPixelValue(image, x, y, chan);
    }
    return sum;
}


int ImageUtils::getPixelValue(IplImage *image, CvPoint point) {

    return getPixelValue(image, point.x, point.y);
}



int ImageUtils::getPixelValue(IplImage *image, int x, int y, int channel) {
    assert(channel < image->nChannels);

    return (((uchar*)(image->imageData + image->widthStep*(y))))[(x * image->nChannels) + channel] ;
}


int ImageUtils::getPixelValue(IplImage *image, CvPoint point, int channel) {
    assert(channel < image->nChannels);

    return getPixelValue(image, point.x, point.y, channel);
}


Pixel ImageUtils::getPixel(IplImage *image, int x, int y, int numChannels) {
    assert(numChannels <= 4);
    assert(numChannels > 0);
    assert(y >= 0 && x >= 0);
    assert(y < image->height && x < image->width);


    int channelValues[4];
    //memset(channelValues, 0, 4);
	channelValues[0] = 0;
	channelValues[1] = 0;
	channelValues[2] = 0;
	channelValues[3] = 0;
	
    for (int i = 0; i < numChannels; i++) {
        channelValues[i] = getPixelValue(image, x, y, i);
    }

    Pixel ret(x, y, channelValues[2], channelValues[1], channelValues[0], channelValues[3]);

    return  ret;


}


Pixel ImageUtils::getPixel(IplImage *image, CvPoint point, int numChannels) {
    return getPixel(image, point.x, point.y, numChannels);
}

CvScalar ImageUtils::getMarkerColor(int index) {

	if (index < 0){
		Debug::printBacktrace(Debug::CRITICAL);	
	}

    //assert(index >= 0);

    index %= MAX_MARKER_COLORS; //silently cope with overflows

    return markerColors(index);

}




bool ImageUtils::isMarkerColor(int r, int g, int b) {

    for (int i = 0; i < MAX_MARKER_COLORS; i++) {
        if ((markerColors(i).val[0] == r) &&
                (markerColors(i).val[1] == g) &&
                (markerColors(i).val[2] == b)) {
            return true;
        }
    }

    return false;
}


void ImageUtils::drawArrow(IplImage* image, CvPoint start, CvPoint end, CvScalar color) {
    ///TODO: irgendwann
    Debug::message("ImageUtils::drawArrow: not implemented");
}



float** ImageUtils::convertImageToResizedGrey( IplImage* img, int newWidth, int newHeight ) {
	
	//Debug::message("img->w: %d, img->h: %d, w/h:%d", Debug::LEVEL_DEBUG, img->width, img->height, newWidth, newHeight);

    float ** pixels;
    //IplImage *img = cvLoadImage( filename.c_str(), 0 );
    IplImage *resized = cvCreateImage( cvSize( newWidth, newHeight ), 8, 1 );
    cvZero(resized);
    
    if (img->nChannels != 1){
    	IplImage* greyTmp = cvCreateImage(cvSize(img->width, img->height), 8,1);
    	cvZero(greyTmp);
    	
    	cvCvtColor(img, greyTmp, CV_BGR2GRAY);
    	//cvEqualizeHist(greyTmp, greyTmp);
    	cvResize(greyTmp, resized, CV_INTER_CUBIC);
    	
    	cvReleaseImage(&greyTmp);	
    } else {
    	//cvEqualizeHist(img, img);
    	cvResize(img, resized, CV_INTER_CUBIC);
    }
    pixels = convertIplImageToFloat( resized );

    //cvReleaseImage( &img );
    cvReleaseImage( &resized );

    return pixels;
}

float** ImageUtils::convertIplImageToFloat( IplImage *img ) {

    uchar *dataImg = ( uchar* )img-> imageData;
    int step = img-> widthStep;
    int w = img->width;
    int h = img->height;

    float **image;
    image = new float*[ w ];
    for( int i=0; i<w; i++ ) {
        image[i] = new float[h];
        for( int j=0; j<h; j++ ) {
            float val = (float)((uchar*)( dataImg + step*(j)))[i] / 255.0;
            image[i][j] = val;
        }
    }

    return image;
}

IplImage* ImageUtils::getSubImage(IplImage* src, CvPoint start, CvPoint end) {
    int newWidth = end.x - start.x;
    int newHeight = end.y - start.y;

    IplImage* subImage = cvCreateImage(cvSize(newWidth, newHeight), src->depth, src->nChannels);
	cvZero(subImage);

    int pixelValue;

    for (int x = 0; x < newWidth; ++x) {
        for (int y = 0; y < newHeight; ++y) {
            for (int channel = 0; channel < src->nChannels; ++channel) {
                pixelValue = getPixelValue(src, x + start.x, y + start.y, channel);
                setPixelValue(subImage, x, y, channel, pixelValue);
            }
        }
    }

    return subImage;

}

IplImage* ImageUtils::getSubImageAsGrey(IplImage* src, CvPoint start, CvPoint end) {
    int newWidth = end.x - start.x;
    int newHeight = end.y - start.y;

    IplImage* subImage = cvCreateImage(cvSize(newWidth, newHeight), src->depth, 1);
	cvZero(subImage);

    float pixelValue;

    for (int x = 0; x < newWidth; ++x) {
        for (int y = 0; y < newHeight; ++y) {

            pixelValue  = (getPixelValue(src, x + start.x, y + start.y, 2)) * 0.114f;
            pixelValue += (getPixelValue(src, x + start.x, y + start.y, 1)) * 0.587f;
            pixelValue += (getPixelValue(src, x + start.x, y + start.y, 0)) * 0.299f;
            setPixelValue(subImage, x, y, 0, (int)ImageUtils::fitRange(pixelValue, 0.0f, 255.0f));

        }
    }

    return subImage;

}


//void ImageUtils::findContour(Blob* blob){
//	
//	IplImage* contour_tmp;
//	CvMemStorage* memStorage;
//	CvSeq* contour;
//	int contourLength; 
//	
//	
//	if (blob->getBoundingRect().width > 1 && blob->getBoundingRect().height > 1){
//	
//		contour_tmp = cvCreateImage(blob->getImageSize(), 8, 1);
//		cvZero(contour_tmp);
//		memStorage = cvCreateMemStorage(0);
//		
//		blob->addToMask(contour_tmp);
//		cvSetImageROI(contour_tmp, blob->getBoundingRect());
//		
//		
//		cvFindContours(contour_tmp, memStorage, &contour, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE, blob->topLeft);
//		
//		int iteration = 0;
//		if (contour != NULL){
//			contourLength = (int)(cvArcLength(contour));
//			
//			/* DEBUG */ contourLength = 30000;
//			
//			CvPoint contourPoints[contourLength];
//			for( ; contour != 0; contour = contour->h_next ){
//				cvCvtSeqToArray(contour, contourPoints);
//				blob->setContour(contourPoints, contourLength, iteration);
//				iteration++;
//			}
//			
//		} 
//		cvReleaseMemStorage(&memStorage);
//		cvReleaseImage(&contour_tmp);
//		//delete [] contourPoints;
//	}
//}


CvScalar ImageUtils::getColorDifference(CvScalar colorA, CvScalar colorB){
	
	return cvScalar(fabs(colorA.val[0] - colorB.val[0]),
					fabs(colorA.val[1] - colorB.val[1]),
					fabs(colorA.val[2] - colorB.val[2]),
					fabs(colorA.val[3] - colorB.val[3]));
}


/* finds the minimum surrounding ellipse for the blob, stores it
 * and returns it */
//CvBox2D ImageUtils::findEnclosingEllipse(Blob* blob){
//	
//	CvMemStorage* memStorage;
//	CvSeq*	contourSeq;
//	
//	//CvMat* mat = cvCreateMat(blob->getImageWidth(), blob->getImageHeight(), CV_32SC1);
//	
//	CvBox2D ellipse;
//	
//	if (blob->m_contourPoints.size() >= 6){
//	
//		memStorage = cvCreateMemStorage(0);
//		contourSeq = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint), memStorage);
//		
//		for (unsigned int i = 0; i < blob->m_points.size(); ++i){
//			cvSeqPush(contourSeq, &(blob->m_points.at(i)));	
//		}
//		
//		ellipse = cvFitEllipse2(contourSeq);
//		
//		blob->setEllipse(ellipse);
//		
//		cvReleaseMemStorage(&memStorage);
//		
//		/*for (unsigned int i = 0; i < blob->m_points.size(); ++i){
//			cvmSet(mat, blob->m_points.at(i).x, blob->m_points.at(i).y, 255.0); 
//		}
//		
//		ellipse = cvFitEllipse2(mat);
//		
//		blob->setEllipse(ellipse);*/
//		
//		
//	} else {
//		Debug::message("Contour size too small, blob id %d", Debug::LEVEL_DEBUG, blob->id);
//		ellipse.center = cvPoint2D32f(0.0,0.0);
//		ellipse.size = 	cvSize2D32f(0.0,0.0);
//		ellipse.angle = 0.0;
//	}
//	
//	return ellipse;
//}
	
	
void ImageUtils::addEllipseEnclosedPixels(IplImage* image, IplImage* src, CvPoint start, CvPoint end, CvBox2D ellipse){
	
	assert(image->nChannels == src->nChannels);
	if (!(ellipse.center.x == 0 && ellipse.center.y == 0)) {
		
		
		IplImage* mask = cvCreateImage(cvSize(src->width, src->height), 8, 1);
		cvZero(mask);
		
		cvEllipse(mask, cvPointFrom32f(ellipse.center),
				  cvSize((int)ellipse.size.width/2, (int)ellipse.size.height/2),
				  -(ellipse.angle), 0, 360, CV_RGB(255,255,255));
		
		int rowFirst;
		int rowLast;
		bool firstFound;
		bool lastFound;
		
		for (int y  = 0; y < src->height; ++y){
			
			firstFound = false;
			lastFound = false;
			
			rowFirst = start.x;
			rowLast = end.x;
			
			for (int x = 0; x < mask->width; ++x){
				if (getPixelValue(mask, x, y) > 0){
					firstFound = true;
					rowFirst = x;
					break;
				}
			}
			
			for (int x = mask->width-1; x >= 0 ; --x){
				if (getPixelValue(mask, x, y) > 0){
					lastFound = true;
					rowLast = x;
					break;
				}
			}
			
			if (firstFound/* && lastFound*/){
				//Debug::message("First: %d, Last: %d", Debug::LEVEL_DEBUG, rowFirst, rowLast);			
				if( y >= start.y && y < end.y){
					for (int x = rowFirst; x <= rowLast; ++x){
						if (x >= start.x && x < end.x){
							for (int chan = 0; chan < image->nChannels; ++chan){
								setPixelValue(image, x - start.x, y - start.y, chan, getPixelValue(src, x, y, chan));	
							}
						}
					}	
				}
			}
		}
		cvReleaseImage(&mask);
	}
	
}
	

	
bool ImageUtils::isSkinColorRGB(int b, int g, int r) {

  	//int mini = MathUtils::min(r,g,b);
   // int maxi = MathUtils::max(r,g,b);
   // int maximini = maxi - mini;

   // return((   r >= ConfigManager::skinColorR)
   //        && (g >= ConfigManager::skinColorG)
   //        && (b >= ConfigManager::skinColorB)
   //        /*&& (maximini > ConfigManager::skinColorDifferenceMaxMin)*/
   //        && (abs(r - g) > ConfigManager::skinColorDifferenceRG)
   //        && r >= g
   //        && r >= b
   //       );
	return false; 
}

bool ImageUtils::isSkinColorHSV(int b, int g, int r) {

	/* convert to HSV */
	float r_float;	
	float g_float;	
	float b_float;	
	
	float h;
	float s;
	float v;
	
	r_float = (float)((float)r)/255.0;
	g_float = ((float)g)/255.0;
	b_float = ((float)b)/255.0;
	
	v = ImageUtils::maxi(r_float, g_float, b_float);
	s = (v == 0 ? 0 : ((v - ImageUtils::mini(r_float, g_float, b_float)) / v));
	
	if (v == r_float){
		h =       (g_float - b_float) * 60 / s;	
	} else if (v == g_float) {
		h = 180 + (b_float - r_float) * 60 / s;
	} else { //v == b_float 
		h = 240 + (r_float - g_float) * 60 / s;
	}
	
	if (h < 0){
		h += 360;	
	}
	
	
	//Debug::message("r :%d, g: %d, b: %d", Debug::LEVEL_DEBUG, r, g, b);
	
	//Debug::message("(1) h: %f, s: %f, v: %f", Debug::LEVEL_DEBUG, h, s, v);
	
	s *= 255.0;
	v *= 255.0;
	/* end convert */
	
	int range_h_min;
	int range_h_max;
	int range_s_min;
	int range_s_max;
	
	
	range_h_min = 0;
	range_h_max = 20;
	range_s_min = 87;
	range_s_max = 200;
	
	//Debug::log("(2) h: %f, s: %f, v: %f |||| r: %d, g: %d, b: %d", Debug::LEVEL_DEBUG, h, s, v, r, g, b);
	
	return (	h >= range_h_min && h <= range_h_max
			&&	s >= range_s_min && s <= range_s_max); 
	
}

bool ImageUtils::isSkinColorYCbCr(int b, int g, int r) {

	/* convert to YCbCr */	
	//float y;
	float cb;
	float cr;
	
	//y  =  0.2989f*r + 0.5866f*g + 0.1145f*b + 0.5f;
	cb = -0.1687f*r - 0.3312f*g + 0.500f*b  + 128.0f;
	cr =  0.500f*r  - 0.4183f*g - 0.0816f*b + 128.0f;
	
	//ImageUtils::fitRange(y , 0.0, 255.0);
	ImageUtils::fitRange(cb, 0.0, 255.0);
	ImageUtils::fitRange(cr, 0.0, 255.0);
	/* end convert */
	
	float range_cb_min;
	float range_cb_max;
	float range_cr_min;
	float range_cr_max;
	
	range_cb_min = 110;
	range_cb_max = 141;
	range_cr_min = 130;
	range_cr_max = 155;
	
	return (	cb >= range_cb_min && cb <= range_cb_max
			&&	cr >= range_cr_min && cr <= range_cr_max);
}

 
 void ImageUtils::drawRect(IplImage* image, CvPoint topLeft, CvPoint bottomRight, CvScalar color, bool filled){
 	
 	topLeft.x     = (int)ImageUtils::fitRange(topLeft.x, 0, image->width);
 	topLeft.y     = (int)ImageUtils::fitRange(topLeft.y, 0, image->height);
 	bottomRight.x = (int)ImageUtils::fitRange(bottomRight.x, 0, image->width);
 	bottomRight.y = (int)ImageUtils::fitRange(bottomRight.y, 0, image->height);
 	
 	
 	CvPoint topRight = cvPoint(bottomRight.x, topLeft.y);
	CvPoint bottomLeft = cvPoint(topLeft.x, bottomRight.y);
	
	
	if (filled){
		CvPoint start;
		CvPoint end;
		
		start = cvPoint(topLeft.x, 0);
		end   = cvPoint(bottomRight.x, 0);
		
		for (int y = topLeft.y; y < bottomRight.y; ++y){
			start.y = y;
			end.y = y;
			cvLine(image, start, end, color);
		}
	
	} else {
		cvLine(image, topLeft,    topRight,    color);
		cvLine(image, topLeft,    bottomLeft,  color);
		cvLine(image, topRight,   bottomRight, color);
		cvLine(image, bottomLeft, bottomRight, color);
	}
 	
 }
 
  void ImageUtils::drawRect(IplImage* image, CvRect rect, CvScalar color, bool filled){
 	CvPoint topLeft = cvPoint(rect.x, rect.y);
 	CvPoint bottomRight = cvPoint (rect.x + rect.width, rect.y + rect.height);
 	
	
	topLeft.x     = (int)ImageUtils::fitRange(topLeft.x, 0, image->width);
 	topLeft.y     = (int)ImageUtils::fitRange(topLeft.y, 0, image->height);
 	bottomRight.x = (int)ImageUtils::fitRange(bottomRight.x, 0, image->width);
 	bottomRight.y = (int)ImageUtils::fitRange(bottomRight.y, 0, image->height);
 	
 	CvPoint topRight = cvPoint(bottomRight.x, topLeft.y);
	CvPoint bottomLeft = cvPoint(topLeft.x, bottomRight.y);
	
	
	if (filled){
		CvPoint start;
		CvPoint end;
		
		start = cvPoint(topLeft.x, 0);
		end   = cvPoint(bottomRight.x, 0);
		
		for (int y = topLeft.y; y < bottomRight.y; ++y){
			start.y = y;
			end.y = y;
			cvLine(image, start, end, color);
		}
	
	} else {
		cvLine(image, topLeft,    topRight,    color);
		cvLine(image, topLeft,    bottomLeft,  color);
		cvLine(image, topRight,   bottomRight, color);
		cvLine(image, bottomLeft, bottomRight, color);
	}
 	
 }
 
 
 void ImageUtils::createMask(IplImage* image, IplImage* mask){
 	assert(image->width == mask->width);	
 	assert(image->height == mask->height);
 	assert(mask->nChannels == 1);
 	
 	for (int x = 0; x < image->width; ++x){
 		for (int y = 0; y < image->height; ++y){
 			if (getPixelValue(image, x, y) > 0){
 				setPixelValue(mask, x, y, 0, 255);	
 			}		
 		}
 	}	
 	
 }
 
 void ImageUtils::copyByMask(IplImage* src, IplImage* dst, IplImage* mask){
 	assert(src->width == mask->width);	
 	assert(src->height == mask->height);
 	assert(src->width == dst->width);	
 	assert(src->height == dst->height);
 	assert(src->nChannels == dst->nChannels);
 	
 	for (int x = 0; x < src->width; ++x){
 		for (int y = 0; y < src->height; ++y){
 			if (getPixelValue(mask, x, y) > 0){
 				setPixel(dst, getPixel(src, x, y, src->nChannels));	
 			}		
 		}
 	}
 	
 }
 
 
void ImageUtils::drawBox2D(IplImage* image, CvBox2D box, CvScalar color){
	
	
	CvPoint pt1, pt2, pt3, pt4;
    CvPoint2D32f boxPoints[4];

    cvBoxPoints(box, boxPoints);

    pt1 = cvPointFrom32f(boxPoints[0]);
    pt2 = cvPointFrom32f(boxPoints[1]);
    pt3 = cvPointFrom32f(boxPoints[2]);
    pt4 = cvPointFrom32f(boxPoints[3]);

    /*pt1.x += topLeft.x;
    pt1.y += topLeft.y;
    pt2.x += topLeft.x;
    pt2.y += topLeft.y;
    pt3.x += topLeft.x;
    pt3.y += topLeft.y;
    pt4.x += topLeft.x;
    pt4.y += topLeft.y;*/


    cvLine(image, pt1, pt2, color);
    cvLine(image, pt1, pt4, color);
    cvLine(image, pt2, pt3, color);
    cvLine(image, pt3, pt4, color);
  
} 


void ImageUtils::drawBox2DFilled(IplImage* image, CvBox2D box, CvScalar color){
	
	
	/*CvPoint pt1, pt2, pt3, pt4;
    CvPoint2D32f boxPoints[4];

    cvBoxPoints(box, boxPoints);

    pt1 = cvPointFrom32f(boxPoints[0]);
    pt2 = cvPointFrom32f(boxPoints[1]);
    pt3 = cvPointFrom32f(boxPoints[2]);
    pt4 = cvPointFrom32f(boxPoints[3]);*/

    IplImage* mask = getMaskFromCvBox2D(box, image->width, image->height);


	for(int x = 0; x < image->width; ++x){
		for(int y = 0; y < image->height; ++y){
			if(getPixelValue(mask, x, y) > 0){
				for (int chan = 0; chan < image->nChannels; ++chan){
					setPixelValue(image, x, y, chan, (int)(color.val[chan]));	
				}			
			}
		}
	}
	
} 

/*  mask MUST be released by calling function */
IplImage* ImageUtils::getMaskFromCvBox2D(CvBox2D box, int width, int height){
	
	IplImage* mask = cvCreateImage(cvSize(width, height), 8, 1);
	cvZero(mask);
	
	drawBox2D(mask, box, cvScalarAll(255));
	
	cvFloodFill(mask, cvPointFrom32f(box.center), cvScalarAll(255));
		
	return mask;
}

//void ImageUtils::drawContour(IplImage* image, Blob* blob, bool filled){
//	
//	IplImage* contour_tmp;
//	CvMemStorage* memStorage;
//	CvSeq* contour;
//	int contourLength; 
//	
//	
//	if (blob->getBoundingRect().width > 1 && blob->getBoundingRect().height > 1){
//	
//		contour_tmp = cvCreateImage(blob->getImageSize(), 8, 1);
//		cvZero(contour_tmp);
//		memStorage = cvCreateMemStorage(0);
//		
//		blob->addToMask(contour_tmp);
//		cvSetImageROI(contour_tmp, blob->getBoundingRect());
//		
//		
//		cvFindContours(contour_tmp, memStorage, &contour, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE, blob->topLeft);
//		
//		int iteration = 0;
//		if (contour != NULL){
//			contourLength = (int)(cvArcLength(contour));
//			
//			/* DEBUG */ contourLength = 30000;
//			
//			CvPoint contourPoints[contourLength];
//			for( ; contour != 0; contour = contour->h_next ){
//				/*cvCvtSeqToArray(contour, contourPoints);
//				blob->setContour(contourPoints, contourLength, iteration);
//				iteration++;*/
//				if (filled) {
//					cvDrawContours(image, contour, CV_RGB(255,255,255), CV_RGB(255,255,255), -1, CV_FILLED);
//				} else {
//					cvDrawContours(image, contour, CV_RGB(255,255,255), CV_RGB(255,255,255), -1);
//				} 
//			}
//			
//		} 
//		cvReleaseMemStorage(&memStorage);
//		cvReleaseImage(&contour_tmp);
//	
//	}
//}
 

bool ImageUtils::overlaps(CvRect a, CvRect b){
	
	CvPoint topLeft, bottomRight, otherTopLeft, otherBottomRight;
	
	topLeft = cvPoint(a.x, a.y);
	bottomRight = cvPoint(a.x + a.width, a.y + a.height);
	otherTopLeft = cvPoint(b.x, b.y);
	otherBottomRight = cvPoint(b.x + b.width, b.y + b.height);
	int distance = 0;
	
	if ((    topLeft.x     >= otherTopLeft.x	 - distance	/// |-------| [other]
          && topLeft.x     <= otherBottomRight.x + distance	/// |       |
          && topLeft.y     >= otherTopLeft.y	 - distance	/// |    |--|-----| [this]
          && topLeft.y     <= otherBottomRight.y + distance	/// |    |  |     |
          													/// |----|--|     |
          													///      |        |
       )) {													///      |--------|
        return true;
    } else if ((   bottomRight.x >= otherTopLeft.x	   - distance ///       |--------| [other]
                && topLeft.x	 <= otherTopLeft.x	   + distance ///       |        |
                && topLeft.y     >= otherTopLeft.y	   - distance /// |-----|-- - - -| -
                && topLeft.y     <= otherBottomRight.y + distance /// |     |        |
                												  /// |     |--------|
                												  ///
              )) {												  /// | [this]
        return true;
    } else if ((   												  /// |--------| [this]
                												  /// |        |
                   bottomRight.x >= otherTopLeft.x	   - distance /// |     |--|-----|
                && bottomRight.x <= otherBottomRight.x + distance /// |     |  |     |
                && bottomRight.y >= otherTopLeft.y	   - distance /// |-----|--|     |
                && bottomRight.y <= otherBottomRight.y)+ distance ///       |        |
              ) {												  ///       |--------| [other]
        return true;
    } else if ((   topLeft.x     >= otherTopLeft.x	   - distance ///       |---- - - - [this]
                && topLeft.x     <= otherBottomRight.x + distance ///       |
                												  /// |-----|--|
                && topLeft.y	 <= otherTopLeft.y     + distance /// |     |  |
                												  /// |        |
                && bottomRight.y >= otherTopLeft.y)    - distance /// |     |  |
              ) {												  /// |--------| [other]
        return true;
    } else if (   topLeft.x     <= otherTopLeft.x	   + distance /// |------------| 
               && topLeft.y     <= otherTopLeft.y	   + distance /// |   [other]  |
               && bottomRight.x >= otherTopLeft.x	   - distance /// |  |------|  |
               && bottomRight.y >= otherBottomRight.y  - distance /// |  |      |  |
                  												  /// |  |      |  |
                  												  /// |  |------|  |
              ) {												  /// |------------| [this]
        return true;
    } else if (   topLeft.x     >= otherTopLeft.x	   + distance /// |------------| 
               && topLeft.y     >= otherTopLeft.y	   + distance /// |   [this]   |
               && bottomRight.x <= otherTopLeft.x	   - distance /// |  |------|  |
               && bottomRight.y <= otherBottomRight.y  - distance /// |  |      |  |
                  												  /// |  |      |  |
                  												  /// |  |------|  |
              ) {												  /// |------------| [other]
        return true;
    }
	
	return false;
	
}

/*double ImageUtils::getOverlap(CvRect a, CvRect b){
	
	double overlap;
	
	
		if (overlaps(a,b)){
			
			CvPoint topLeft, bottomRight, otherTopLeft, otherBottomRight;
			int distance = 0;
	
			topLeft = cvPoint(a.x, a.y);
			bottomRight = cvPoint(a.x + a.width, a.y + a.height);
			otherTopLeft = cvPoint(b.x, b.y);
			otherBottomRight = cvPoint(b.x + b.width, b.y + b.height);
			
			if (   topLeft.x     <= otherTopLeft.x	   + distance /// |------------| 
	               && topLeft.y     <= otherTopLeft.y	   + distance /// |   [other]  |
	               && bottomRight.x >= otherTopLeft.x	   - distance /// |  |------|  |
	               && bottomRight.y >= otherBottomRight.y  - distance /// |  |      |  |
	                  												  /// |  |      |  |
	                  												  /// |  |------|  |
	              ) {												  /// |------------| [this]
	        	overlap = 1.0;
		    } else if (   topLeft.x     >= otherTopLeft.x	   + distance /// |------------| 
		               && topLeft.y     >= otherTopLeft.y	   + distance /// |   [this]   |
		               && bottomRight.x <= otherTopLeft.x	   - distance /// |  |------|  |
		               && bottomRight.y <= otherBottomRight.y  - distance /// |  |      |  |
		                  												  /// |  |      |  |
		                  												  /// |  |------|  |
		              ) {												  /// |------------| [other]
		        overlap = 1.0;
		    } else {
		
				//TODO da gibts bestimmt ne elegantere Moeglichkeit, aber mir fehlt die Zeit, und diese hier passt auch
				
				int blackCountA = 0;
				int whiteCountA = 0;
				int blackCountB = 0;
				int whiteCountB = 0;
				
				//int width  = abs(MathUtils::max(a.x + a.width,  b.x + b.width)  - MathUtils::min(a.x, b.x));
				//int height = abs(MathUtils::max(a.y + a.height, b.y + b.height) - MathUtils::min(a.y, b.y));
				
				
				IplImage* mask = cvCreateImage(ConfigManager::imageSize, 8, 1);
				cvZero(mask);
				drawRect(mask, b, cvScalarAll(255), true);
				for (int y = a.y; y < a.y + a.height; ++y){
					for (int x = a.x; x < a.x + a.width; ++x){
						if(getPixelValue(mask, x, y, 0) > 0){
							whiteCountA++;
						} else {
							blackCountA++;
						}
						//setPixelValue(mask, x, y, 0, 0);
					}
				}
				
				cvZero(mask);
				drawRect(mask, a, cvScalarAll(255), true);
				for (int y = b.y; y < b.y + b.height; ++y){
					for (int x = b.x; x < b.x + b.width; ++x){
						if(getPixelValue(mask, x, y, 0) > 0){
							whiteCountB++;
						} else {
							blackCountB++;
						}
						//setPixelValue(mask, x, y, 0, 0);
					}
				}
				 
				cvReleaseImage(&mask); 
				
				Debug::message("overlap 1: %f, overlap 2: %f", Debug::DEBUG, ((float)whiteCountA / (float)(whiteCountA + blackCountA)), ((float)whiteCountB / (float)(whiteCountB + blackCountB)));
				
				overlap = MathUtils::max(((float)whiteCountA / (float)(whiteCountA + blackCountA)), ((float)whiteCountB / (float)(whiteCountB + blackCountB)));
			}
	} else {
		overlap = 0.0;	
	}
	Debug::message("overlap: %f", Debug::DEBUG, overlap);
	
	return overlap;
}*/

